#ifndef _SERVO_UBT_H_
#define _SERVO_UBT_H_
#include "ServoBase.h"
#include "UbtProtocol.h"


// Tx enbale pin for one-line bus
#define TXEN_PIN					4


/**
* @brief:
* UBT bus servo, eg: 9kg/12kg/...
*/
class CServoUBT : public CServoBus
{
public:
	CServoUBT();
	virtual ~CServoUBT();

	virtual bool IsOffsetSupport();
	virtual ServoRet_t SetAngle(uint16_t angle, uint32_t msec);
	virtual ServoRet_t GetAngle(uint16_t &angle);
	virtual ServoRet_t SetOffset(int16_t offset);
	virtual ServoRet_t GetOffset(int16_t &offset);
	virtual ServoRet_t SetId(uint8_t newid);
	virtual ServoRet_t SetLed(bool on);
	virtual ServoRet_t Terminate();
	virtual ServoRet_t GetFirmware(uint32_t &firmware);
	virtual ServoRet_t Lock();
	virtual ServoRet_t Unlock();

protected:
	void prepareSnd();
	void prepareRcv();
	ServoRet_t sendCommand(uint8_t *cmd, int expect, uint8_t *out);
};


/**
 * @brief:
 * UBT servo raw cmds executing
 *
 * The command is passed from up to servo and returned back from servo to up!
 * Default waiting time of command is 10ms(400us is enough theoretically)!
 */
class CUBTServoExec
{
public:
	CUBTServoExec();
	~CUBTServoExec();

	static ServoRet_t Execute(HardwareSerial *serl, uint8_t *cmd, int &expect, uint8_t *out);
};


#endif

